#include "chassis_node.hpp"
#include "support-chassis/config.hpp"

#include <log/log.hpp>
#include <support-mqtt/mqtt.hpp>
#include <commons/net/http_server.hpp>
#include <commons/sync_async/async_timer.hpp>

namespace sros::control 
{

chassisNode::chassisNode(const std::string &name, const std::string &module)
    : Node(name, module)
    , cfg_(nullptr)
    , timer_(nullptr)
    , mqtt_(nullptr)
    , http_server_(nullptr)
    , target_spd_(0)
{
    
}

RetBool chassisNode::init()
{
    std::string root = util::env_var("HOME");
    if (root.empty())
    {
        root = ".";
    }

    std::string log_path = util::format("{}/.sros/{}/{}/log", root, info().module, info().name);
    std::string log_filename = util::format("{}.log", info().name);
    Log::Param log_param;
    log_param.path = log_path;
    log_param.file_name = log_filename;
    Log::ins().init(log_param);

    LOG_INFO("-------------init-------------");

    std::string config_path = util::format("{}/.sros/{}/{}/config/config.json", root, info().module, info().name);
    
    cfg_ = std::make_shared<commons::Config<Cfg>>();
    auto ret = cfg_->init(config_path);
    if (!ret.v)
    {
        LOG_WARN("cfg init failed, {}", ret.msg);
        return ret;
    }
    
    ret = cfg_->load();
    if (!ret.v)
    {
        LOG_WARN("cfg load failed, {}", ret.msg);
        return ret;
    }

    // mqtt 客户端初始化
    plugins::MqttParam mqtt_pram;
    mqtt_pram.id = info().module + "_" + info().name;
    mqtt_pram.keepalive = 10;
    mqtt_pram.on_error = [](const int &&ec, const std::string &&msg)
    {
        LOG_ERROR("[MQTT] ec:{} msg:{}", ec, msg);
    };
    mqtt_pram.on_status_changed = [](const int &&s1, const std::string &&m1, const int &&s2, const std::string &&m2)
    {
        LOG_INFO("[MQTT] [{},{}]->[{},{}]", s1, m1, s2, m2);
    };
    mqtt_pram.remote_ip = "0.0.0.0";
    mqtt_pram.remote_port = 1883;
    mqtt_pram.reconnect_delay = 1;
    mqtt_pram.reconnect_delay_max = 10;
    mqtt_pram.reconnect_exponential_backoff = true;
    mqtt_ = std::make_shared<sros::plugins::Mqtt>(mqtt_pram);

    timer_ = std::make_shared<async::Timer>();
    
    EncoderStatus encoder_status = {0};
    encoder_list_.push_back(encoder_status);
    encoder_list_.push_back(encoder_status);

    http_server_ = std::make_shared<net::HttpServer>("0.0.0.0", 6666);

    http_server_->handle_post("/sros/control/chassis/spd/set", [this](const httplib::Request &, httplib::Response &rsp)
    {
        // rsp.set_content("", "application/json");
    });

    return {true};
}

void chassisNode::run()
{
    LOG_INFO("-------------running-------------");

    http_server_->start();

    mqtt_->run();

    // 订阅外置编码器的数据
    mqtt_->sub("/sros/device/enc_ab/status", [this](const std::vector<uint8_t> &&buffer)
    {
        try
        {
            encoder_list_ = nlohmann::json::parse(buffer).get<std::vector<EncoderStatus>>();
        }
        catch (const std::exception &e)
        {
            LOG_ERROR("[MQTT] parse /sros/device/enc_ab/status failed, {}", e.what());
        }
    });

    timer_->safe_start(10, 40, 0, [this](std::size_t times)
    {
        if (times <= 50)
        {
            DriverCmd cmd {.dir = H, .cmd = SET_SPD, .data = 100};
            
            nlohmann::json j = cmd;
            std::string cont = j.dump();
            std::vector<uint8_t> payload(cont.begin(), cont.end());

            mqtt_->pub("/sros/device/driver/cmd", payload);
        }
        if (times > 50)
        {
            DriverCmd cmd {.dir = H, .cmd = SET_SPD, .data = 0};
            
            nlohmann::json j = cmd;
            std::string cont = j.dump();
            std::vector<uint8_t> payload(cont.begin(), cont.end());

            mqtt_->pub("/sros/device/driver/cmd", payload);
        }
        position_loop_();
        speed_loop_();
    });

    http_server_->start();
}

void chassisNode::close()
{
    LOG_INFO("-------------close-------------");

    if (http_server_)
    {
        http_server_->stop();
    }

    if (timer_)
    {
        timer_->safe_stop();
        timer_->safe_clear();
    }

    if (mqtt_)
    {
        mqtt_->close();
    }

    if (cfg_)
    {
        cfg_->deinit();
    }

    Log::ins().deinit();
}

void chassisNode::position_loop_()
{
//     // 计算位置误差
// 	int32_t pos = encoder_list_[H].pos;
//     int32_t error = (spd_ctl.target_rpm - actual_rpm);
//     float P = spd_ctl.Kp * error;


//     // 2. 计算积分项,只有输出没有达到最大输出值的时候,才进行积分累计,防止积分过饱和
//     if ( !((spd_ctl.output_ma >= spd_ctl.max_current && reverse*error > 0) || (spd_ctl.output_ma <= -spd_ctl.max_current && reverse*error < 0)) )
//     {
//     	spd_ctl.integral += (error * spd_ctl.time_delta);
//     }
//     float I = spd_ctl.Ki * spd_ctl.integral;

//     // 3. 计算微分项
//     int32_t derivative = (error - spd_ctl.prev_error) / spd_ctl.time_delta;
//     float D = spd_ctl.Kd * derivative;

//     // 4. 合成输出
//     spd_ctl.output_ma = P + I + D;

//     // 5. 输出限幅
//     spd_ctl.output_ma = (spd_ctl.output_ma > spd_ctl.max_current) ? spd_ctl.max_current :
//                         (spd_ctl.output_ma < -spd_ctl.max_current) ? -spd_ctl.max_current :
//                     		 spd_ctl.output_ma;

//     spd_ctl.output_ma = reverse * spd_ctl.output_ma;

// //    Log(LOG_SYSTEM, RTT_BLUE, "spd_ctl output: %dma error:%d, trpm:%d, arpm:%d, arpm2:%d P:%f, I:%f D:%f.\n", spd_ctl.output_ma, error, spd_ctl.target_rpm, actual_rpm, spd_ctl.actual_rpm, P, I, D);

//     Motor_set_current(H, spd_ctl.output_ma);

//     // 6. 保存误差
//     spd_ctl.prev_error = error;
}

void chassisNode::speed_loop_()
{

}

}

